#include "MeasurE616AlgFrontS1S2Edge.h"

MeasurE616AlgFrontS1S2Edge::MeasurE616AlgFrontS1S2Edge(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgFrontS1S2Edge::operator()(cv::Mat &srcImg, int id,std::map<string, float> &measRes
                                            , int fixtureId)
{

    std::vector<std::vector<cv::Point2f>>resptVec;
    resptVec.resize(4);
    {
        int i=36;
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(0,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE616EdgeDetect obj;
        obj.NewKirschEdgeOuter(imgt, tparas, contours);
        ImageCoordinateTransformation(contours,rangVec);
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrfrontVec[id]);
        while (!MeasurE616AlgBase::flagfrontVec[id])
        {
            (*MeasurE616AlgBase::cvarptrfrontVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformationRelateToDot(datumFrontVec[id], ptsIn, ptsOut);
            resptVec[0].push_back(ptsOut);
        }

    }


    {
        int i=36;
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(1,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE616EdgeDetect obj;
        obj.NewKirschEdgeOuter(imgt, tparas, contours);
        ImageCoordinateTransformation(contours,rangVec);
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrfrontVec[id]);
        while (!MeasurE616AlgBase::flagfrontVec[id])
        {
            (*MeasurE616AlgBase::cvarptrfrontVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformationRelateToDot(datumFrontVec[id], ptsIn, ptsOut);
            resptVec[1].push_back(ptsOut);
        }

    }


    {
        int i=37;
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(2,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE616EdgeDetect obj;
        obj.NewKirschEdgeOuter(imgt, tparas, contours);
        ImageCoordinateTransformation(contours,rangVec);
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrfrontVec[id]);
        while (!MeasurE616AlgBase::flagfrontVec[id])
        {
            (*MeasurE616AlgBase::cvarptrfrontVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformationRelateToDot(datumFrontVec[id], ptsIn, ptsOut);
            resptVec[2].push_back(ptsOut);
        }

    }


    {
        int i=37;
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(3,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE616EdgeDetect obj;
        obj.NewKirschEdgeOuter(imgt, tparas, contours);
        ImageCoordinateTransformation(contours,rangVec);
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrfrontVec[id]);
        while (!MeasurE616AlgBase::flagfrontVec[id])
        {
            (*MeasurE616AlgBase::cvarptrfrontVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformationRelateToDot(datumFrontVec[id], ptsIn, ptsOut);
            resptVec[3].push_back(ptsOut);
        }

    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI45-2-S1-X-distance_pixel"), fabs(resptVec[0][0].x-resptVec[1][0].x)));
    float fai452 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI45-2-S1-X-distance")], fabs(resptVec[0][0].x-resptVec[1][0].x));
    measRes.insert(std::make_pair(string("FAI45-2-S1-X-distance_real"), fai452));

    measRes.insert(std::make_pair(string("FAI45-2-S2-X-distance_pixel"), fabs(resptVec[2][0].x-resptVec[3][0].x)));
    float fai4522 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI45-2-S2-X-distance")], fabs(resptVec[2][0].x-resptVec[3][0].x));
    measRes.insert(std::make_pair(string("FAI45-2-S2-X-distance_real"), fai4522));

}

